#!/usr/bin/env python3
# 指定脚本使用Python 3解释器执行

# 导入必要的库
import rclpy  # ROS 2 Python客户端库，用于创建和管理节点
from rclpy.node import Node  # ROS 2节点基类，所有自定义节点都需继承此类
from livox_interfaces.msg import CustomMsg  # Livox激光雷达的自定义点云消息类型
from sensor_msgs.msg import PointCloud2, PointField  # ROS标准点云消息及字段定义
from geometry_msgs.msg import PoseStamped  # 用于发布目标姿态信息
from std_msgs.msg import Bool  # 用于发布碰撞检测结果
import numpy as np  # 用于高效的数值计算和数组操作
import struct  # 用于二进制数据的打包与解包，构建PointCloud2消息时使用
import open3d as o3d  # 用于3D点云处理、可视化和聚类分析
from threading import Thread, Lock  # 多线程支持，Lock用于线程间同步
import time  # 用于计时和控制程序执行速度
import multiprocessing  # 用于并行计算，提高点云处理效率

# 定义PointCloud2消息的字段结构，描述点云数据的组成
FIELDS = [
    # X坐标：偏移0字节，32位浮点数，数量1
    PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1),
    # Y坐标：偏移4字节（紧跟X之后），32位浮点数，数量1
    PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1),
    # Z坐标：偏移8字节（紧跟Y之后），32位浮点数，数量1
    PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1),
    # 反射率：偏移12字节（紧跟Z之后），8位无符号整数，数量1
    PointField(name='intensity', offset=12, datatype=PointField.UINT8, count=1),
]
POINT_STEP = 13  # 每个点占用的字节数：3个float(4字节×3) + 1个uint8(1字节) = 13字节
ROW_STEP = 0  # 每行的字节数，后续会根据点云数量动态计算


def process_chunk(chunk, distance_threshold, keep_front, area_half_width, area_half_height, 
                 exclude_area_params):
    """
    并行处理点云数据块的过滤函数，实现距离过滤、区域选择和特定区域屏蔽
    """
    # 1. 距离过滤：计算每个点到原点的平方距离（避免开平方运算提高效率）
    squared_distances = np.sum(chunk[:, :3] **2, axis=1)
    distance_mask = squared_distances <= (distance_threshold** 2)
    
    # 2. 前方区域过滤：仅保留传感器正前方特定范围内的点
    if keep_front:
        in_front_mask = (chunk[:, 0] > 0)
        in_width_mask = (np.abs(chunk[:, 1]) <= area_half_width)
        in_height_mask = (np.abs(chunk[:, 2]) <= area_half_height)
        
        front_area_mask = np.logical_and(np.logical_and(in_front_mask, in_width_mask), in_height_mask)
        filter_mask = np.logical_and(distance_mask, front_area_mask)
    else:
        filter_mask = distance_mask
    
    # 3. 排除指定区域内的点
    in_exclude_area = (
        (chunk[:, 0] >= exclude_area_params['x_min']) &
        (chunk[:, 0] <= exclude_area_params['x_max']) &
        (chunk[:, 1] >= exclude_area_params['y_min']) &
        (chunk[:, 1] <= exclude_area_params['y_max']) &
        (chunk[:, 2] >= exclude_area_params['z_min']) &
        (chunk[:, 2] <= exclude_area_params['z_max'])
    )
    
    final_mask = np.logical_and(filter_mask, ~in_exclude_area)
    return chunk[final_mask]


class LidarProcessor(Node):
    """LiDAR点云处理器节点类，实现点云数据的接收、处理、发布和可视化"""
    
    def __init__(self):
        """初始化LiDAR处理器节点，设置订阅者、发布者和各种参数"""
        super().__init__('lidar_processor')
        
        # 声明并获取ROS参数：输入点云话题名称
        self.declare_parameter('input_cloud', '/livox/lidar_avia')
        input_topic = self.get_parameter('input_cloud').get_parameter_value().string_value

        # 创建订阅者，订阅Livox激光雷达的自定义点云消息
        self.sub = self.create_subscription(
            CustomMsg, input_topic, self.callback, 10
        )
        
        # 创建发布者，发布处理后的标准PointCloud2格式点云
        self.pcl_pub = self.create_publisher(
            PointCloud2, '/livox/pointcloud2', 10
        )

        # 创建发布者用于发布检测到的长方体点云、姿态和碰撞检测结果
        self.box_pub = self.create_publisher(
            PointCloud2, '/detected_cuboid', 10
        )
        self.pose_pub = self.create_publisher(
            PoseStamped, '/cuboid_pose', 10
        )
        self.collision_pub = self.create_publisher(
            Bool, '/collision_warning', 10
        )
        
        self.get_logger().info(f"订阅: {input_topic}，发布处理后点云到: /livox/pointcloud2")

        # 初始化可视化相关参数和对象
        self.declare_parameter('enable_visualization', True)
        self.enable_visualization = self.get_parameter('enable_visualization').get_parameter_value().bool_value
        
        if self.enable_visualization:
            self.pcd = o3d.geometry.PointCloud()
            self.target_pcd = o3d.geometry.PointCloud()  # 用于显示目标长方体
            self.collision_pcd = o3d.geometry.PointCloud()  # 用于显示碰撞风险聚类
            self.line_set = o3d.geometry.LineSet()       # 用于显示连线
            self.vis = o3d.visualization.Visualizer()
            self.vis.create_window()
            self.vis.get_render_option().point_size = 2.0
            self.vis.get_render_option().background_color = np.array([0, 0, 0])
            self.point_cloud_lock = Lock()
            self.is_visualizing = True
            self.is_first_frame = True
            self.visualization_thread = Thread(target=self.visualization_loop)
            self.visualization_thread.daemon = True
            self.visualization_thread.start()
        else:
            self.get_logger().info("可视化已关闭，将提高处理速度。")
        
        # 点云计数和保存相关参数
        self.frame_count = 0
        self.save_path = "/path/to/save/pointclouds/"
        self.save_enabled = False
        self.use_fixed_color = True
        
        # 过滤参数
        self.distance_threshold = 50.0
        self.keep_front = True
        
        # 前方区域尺寸参数
        self.area_width = 2
        self.area_height = 2
        self.area_half_width = self.area_width / 2
        self.area_half_height = self.area_height / 2
        
        # 聚类分析参数
        self.cluster_eps = 0.1
        self.cluster_min_points = 30
        self.enable_clustering = True
        self.voxel_size = 0.03  
        
        # 聚类点数可视化范围
        self.cluster_min_visualize = 0    # 最小可视化点数
        self.cluster_max_visualize = 8000  # 最大可视化点数
        
        # 长方体识别参数
        self.cuboid_min_length = 0.2    # 最小长度（米）
        self.cuboid_max_length = 0.45    # 最大长度（米）
        self.cuboid_min_width = 0.2     # 最小宽度（米）
        self.cuboid_max_width = 0.45     # 最大宽度（米）
        self.cuboid_min_height = 0.2    # 最小高度（米）
        self.cuboid_max_height = 0.4    # 最大高度（米）
        self.cuboid_aspect_ratio = 1.5  # 最大长宽比（避免细长物体）
        self.get_logger().info(
            f"启用长方体识别：尺寸范围 {self.cuboid_min_length}-{self.cuboid_max_length}m（长），"
            f"{self.cuboid_min_width}-{self.cuboid_max_width}m（宽），"
            f"{self.cuboid_min_height}-{self.cuboid_max_height}m（高）"
        )
        
        # 碰撞检测参数
        self.collision_threshold = 0.1  # 碰撞判定阈值（米）
        self.get_logger().info(
            f"启用碰撞检测：当目标与其他聚类距离小于 {self.collision_threshold}m 时发出警告"
        )
        
        # 并行处理设置
        self.num_processes = max(1, multiprocessing.cpu_count() - 1)
        
        # 排除区域参数
        self.exclude_area_params = {
            'x_min': 0.0,    'x_max': 1,
            'y_min': -1,     'y_max': 1.0,
            'z_min': -2,     'z_max': -0.1
        }
        self.get_logger().info(
            f"已启用区域屏蔽: X∈[{self.exclude_area_params['x_min']},{self.exclude_area_params['x_max']}], "
            f"Y∈[{self.exclude_area_params['y_min']},{self.exclude_area_params['y_max']}], "
            f"Z∈[{self.exclude_area_params['z_min']},{self.exclude_area_params['z_max']}]"
        )
        self.get_logger().info(
            f"只可视化点数在 {self.cluster_min_visualize}-{self.cluster_max_visualize} 范围内的聚类"
        )

    def callback(self, msg: CustomMsg):
        """订阅消息的回调函数，处理接收到的LiDAR点云数据"""
        start_time = time.time()
        
        # 转换点云消息为NumPy数组
        points_array = np.zeros((len(msg.points), 4), dtype=np.float32)
        for i, p in enumerate(msg.points):
            points_array[i, 0] = p.x
            points_array[i, 1] = p.y
            points_array[i, 2] = p.z
            points_array[i, 3] = p.reflectivity
        
        # 并行过滤点云
        chunks = np.array_split(points_array, self.num_processes)
        with multiprocessing.Pool(processes=self.num_processes) as pool:
            results = pool.starmap(
                process_chunk, 
                [
                    (chunk, self.distance_threshold, self.keep_front, 
                     self.area_half_width, self.area_half_height, self.exclude_area_params) 
                    for chunk in chunks
                ]
            )
        filtered_points = np.concatenate(results)
        
        # 处理点云并设置颜色
        target_cuboid_points = None  # 存储目标长方体点云
        target_pose = None           # 存储目标姿态信息
        tilt_angle = 0.0             # 倾斜角度
        target_center = None         # 目标中心点
        other_clusters = []          # 存储其他聚类
        collision_detected = False   # 碰撞检测结果
        collision_clusters = []      # 存储发生碰撞风险的聚类
        
        if len(filtered_points) < 50:
            if self.frame_count % 10 == 0:
                self.get_logger().info(f"过滤后点太少（{len(filtered_points)}点），跳过聚类")
            colors = np.ones((len(filtered_points), 3))  # 白色
        else:
            # 创建Open3D点云对象并降采样
            pcd = o3d.geometry.PointCloud()
            pcd.points = o3d.utility.Vector3dVector(filtered_points[:, :3])
            downsampled_pcd = pcd.voxel_down_sample(voxel_size=self.voxel_size)
            
            if self.enable_clustering:
                # DBSCAN聚类
                labels = np.array(downsampled_pcd.cluster_dbscan(
                    eps=self.cluster_eps, 
                    min_points=self.cluster_min_points, 
                    print_progress=False
                ))
                
                # 统计每个聚类的点数
                unique_labels, counts = np.unique(labels, return_counts=True)
                cluster_sizes = dict(zip(unique_labels, counts))
                
                # 打印所有聚类的点数
                for label, count in cluster_sizes.items():
                    if label == -1:
                        self.get_logger().info(f"噪声点数量: {count}")
                    else:
                        self.get_logger().info(f"聚类 {label} 的点数: {count}")
                
                # 映射聚类标签到原始点云
                pcd_tree = o3d.geometry.KDTreeFlann(downsampled_pcd)
                full_labels = np.ones(len(filtered_points), dtype=np.int32) * -1
                
                for i in range(len(filtered_points)):
                    [k, idx, _] = pcd_tree.search_knn_vector_3d(filtered_points[i, :3], 1)
                    full_labels[i] = labels[idx[0]]
                
                # 生成颜色（包含长方体识别逻辑）
                max_label = full_labels.max()
                self.get_logger().info(f"检测到 {max_label + 1} 个聚类")
                
                colors = np.zeros((len(filtered_points), 3))  # 默认为黑色（不可见）
                visible_clusters = 0
                cuboid_candidates = []  # 存储所有长方体候选聚类
                
                for label in range(-1, max_label + 1):
                    if label == -1:
                        continue  # 跳过噪声点
                        
                    # 获取当前聚类的所有点
                    cluster_mask = full_labels == label
                    cluster_points = filtered_points[cluster_mask][:, :3]
                    cluster_size = cluster_sizes.get(label, 0)
                    
                    # 检查是否在可视化点数范围内
                    if self.cluster_min_visualize <= cluster_size <= self.cluster_max_visualize:
                        # 计算几何特征并判断是否为长方体
                        features = self.calculate_cuboid_features(cluster_points)
                        is_cuboid = self.is_cuboid(features)
                        
                        if is_cuboid:
                            # 计算聚类中心到X轴的距离（Y² + Z²的平方根）
                            cluster_center = np.mean(cluster_points, axis=0)
                            distance_to_x_axis = np.sqrt(cluster_center[1]**2 + cluster_center[2]** 2)
                            
                            # 存储长方体候选及其特征
                            cuboid_candidates.append({
                                'points': cluster_points,
                                'features': features,
                                'distance_to_x': distance_to_x_axis,
                                'center': cluster_center,
                                'mask': cluster_mask,
                                'label': label
                            })
                            
                            # 暂时用蓝色标记所有候选长方体
                            colors[cluster_mask] = [0.0, 0.0, 1.0]
                        else:
                            # 普通聚类：用彩色标记并添加到其他聚类列表
                            hue = label / (max_label + 1)
                            # rgb = self.hsv_to_rgb(hue, 0.8, 0.9)
                            rgb = self.hsv_to_rgb(0.7, 0.8, 0.9)
                            colors[cluster_mask] = rgb
                            
                            other_clusters.append({
                                'points': cluster_points,
                                'center': np.mean(cluster_points, axis=0),
                                'mask': cluster_mask,
                                'label': label
                            })
                            
                        visible_clusters += 1
                    # else:
                    #     self.get_logger().info(
                    #         f"聚类 {label} 点数 {cluster_size} 超出范围，不进行可视化"
                    #     )
                
                # 筛选出距离X轴最近的长方体
                if cuboid_candidates:
                    # 按到X轴的距离排序，取最近的一个
                    cuboid_candidates.sort(key=lambda x: x['distance_to_x'])
                    target_cuboid = cuboid_candidates[0]
                    target_cuboid_points = target_cuboid['points']
                    target_center = target_cuboid['center']
                    target_label = target_cuboid['label']
                    
                    # 用红色标记目标长方体
                    colors[target_cuboid['mask']] = [1.0, 0.0, 0.0]
                    
                    # 计算倾斜角度：长方体中心与激光雷达连线和X轴的夹角
                    tilt_angle = self.calculate_tilt_angle(target_center)
                    
                    # 计算目标姿态
                    target_pose = self.create_pose_message(
                        msg.header, 
                        target_center,
                        tilt_angle
                    )
                    
                    # 执行碰撞检测：检查目标与其他聚类的距离
                    collision_detected, collision_clusters = self.detect_collision(
                        target_cuboid_points, other_clusters
                    )
                    
                    # 标记碰撞风险的聚类为黄色
                    for cluster in collision_clusters:
                        colors[cluster['mask']] = [1.0, 1.0, 0.0]
                    
                    # 发布碰撞检测结果
                    collision_msg = Bool()
                    collision_msg.data = collision_detected
                    self.collision_pub.publish(collision_msg)
                    
                    self.get_logger().info(
                        f"已选择距离X轴最近的长方体 - 距离X轴: {target_cuboid['distance_to_x']:.2f}m, "
                        f"中心坐标: ({target_center[0]:.2f}, {target_center[1]:.2f}, {target_center[2]:.2f}), "
                        f"与X轴夹角: {tilt_angle:.2f}度, "
                    )
                else:
                    self.get_logger().info("未检测到长方体目标")
                    # 发布无碰撞（因为没有目标）
                    collision_msg = Bool()
                    collision_msg.data = False
                    self.collision_pub.publish(collision_msg)
                
                self.get_logger().info(
                    f"可视化聚类数量: {visible_clusters}，检测到长方体候选: {len(cuboid_candidates)}, "
                    f"碰撞风险聚类: {len(collision_clusters)}"
                )
                
            else:
                # 不启用聚类时，基于反射率着色
                intensities = filtered_points[:, 3] / 255.0
                colors = np.zeros((len(filtered_points), 3))
                colors[:, 0] = intensities
        
        # 发布处理后的点云消息
        pcl_msg = self.create_pointcloud2_msg(msg.header, filtered_points)
        self.pcl_pub.publish(pcl_msg)
        
        # 发布目标长方体点云和姿态
        if target_cuboid_points is not None:
            # 发布目标长方体点云
            cuboid_msg = self.create_pointcloud2_msg(
                msg.header, 
                np.hstack([target_cuboid_points, np.zeros((len(target_cuboid_points), 1))])
            )
            self.box_pub.publish(cuboid_msg)
            
            # 发布目标姿态
            if target_pose is not None:
                self.pose_pub.publish(target_pose)
        
        # 更新可视化（仅在开启时执行）
        if self.enable_visualization:
            with self.point_cloud_lock:
                points_np = filtered_points[:, :3]
                
                if self.is_first_frame:
                    x_min, y_min, z_min = np.min(points_np, axis=0)
                    x_max, y_max, z_max = np.max(points_np, axis=0)
                    self.get_logger().info(
                        f"坐标范围: x=[{x_min:.2f}, {x_max:.2f}], "
                        f"y=[{y_min:.2f}, {y_max:.2f}], "
                        f"z=[{z_min:.2f}, {z_max:.2f}]"
                    )
                    
                    scale = 1.0
                    max_coord = max(abs(x_max), abs(x_min), abs(y_max), abs(y_min), abs(z_max), abs(z_min))
                    if max_coord > 1000:
                        scale = 0.001
                        self.get_logger().info(f"应用缩放因子: {scale}")
                        points_np *= scale
                
                self.pcd.points = o3d.utility.Vector3dVector(points_np)
                self.pcd.colors = o3d.utility.Vector3dVector(colors)
                
                # 更新目标长方体点云
                if target_cuboid_points is not None:
                    self.target_pcd.points = o3d.utility.Vector3dVector(target_cuboid_points)
                    self.target_pcd.colors = o3d.utility.Vector3dVector(
                        np.tile([1.0, 0.0, 0.0], (len(target_cuboid_points), 1))
                    )
                
                # 更新碰撞风险聚类点云
                if collision_clusters:
                    collision_points = np.vstack([cluster['points'] for cluster in collision_clusters])
                    self.collision_pcd.points = o3d.utility.Vector3dVector(collision_points)
                    self.collision_pcd.colors = o3d.utility.Vector3dVector(
                        np.tile([1.0, 1.0, 0.0], (len(collision_points), 1))
                    )
                else:
                    # 清空碰撞点云
                    self.collision_pcd.points = o3d.utility.Vector3dVector([])
                
                # 更新激光雷达到目标中心的连线
                if target_center is not None:
                    # 激光雷达位置(原点)到目标中心的连线
                    self.line_set.points = o3d.utility.Vector3dVector([
                        [0, 0, 0],  # 激光雷达位置
                        target_center  # 目标中心
                    ])
                    # 定义线的连接关系
                    self.line_set.lines = o3d.utility.Vector2iVector([[0, 1]])
                    # 设置线的颜色为绿色
                    self.line_set.colors = o3d.utility.Vector3dVector([[0, 1, 0]])
                
                if self.save_enabled:
                    self.save_current_frame()
                
                self.is_first_frame = False
        
        # 记录处理信息
        self.frame_count += 1
        process_time = (time.time() - start_time) * 1000
        self.get_logger().info(
            f"帧 {self.frame_count}: 原始{len(msg.points)}点 → 过滤后{len(filtered_points)}点 → "
            f"处理时间{process_time:.2f}ms"
        )

    def calculate_cuboid_features(self, cluster_points):
        """计算聚类的几何特征，用于判断是否为长方体"""
        # 计算轴对齐最小包围盒
        min_box = np.min(cluster_points, axis=0)  # [x_min, y_min, z_min]
        max_box = np.max(cluster_points, axis=0)  # [x_max, y_max, z_max]
        
        # 计算长宽高（排序后确保长≥宽≥高）
        dimensions = max_box - min_box
        dimensions_sorted = np.sort(dimensions)[::-1]  # 从大到小排序
        length, width, height = dimensions_sorted
        
        # 计算体积和长宽比
        volume = length * width * height
        aspect_ratio = length / width if width != 0 else float('inf')
        
        return {
            "length": length,
            "width": width,
            "height": height,
            "volume": volume,
            "aspect_ratio": aspect_ratio,
            "min_box": min_box,
            "max_box": max_box,
            "center": np.mean(cluster_points, axis=0)
        }

    def is_cuboid(self, features):
        """判断聚类是否符合长方体特征"""
        return (
            # 长度在合理范围
            self.cuboid_min_length <= features["length"] <= self.cuboid_max_length and
            # 宽度在合理范围
            self.cuboid_min_width <= features["width"] <= self.cuboid_max_width and
            # 高度在合理范围
            self.cuboid_min_height <= features["height"] <= self.cuboid_max_height and
            # 长宽比不过大（避免细长物体）
            features["aspect_ratio"] <= self.cuboid_aspect_ratio
        )

    def calculate_tilt_angle(self, center):
        """计算长方体中心与激光雷达连线（原点到中心）和X轴的夹角（度）"""
        # 向量为从激光雷达(原点)到长方体中心
        vector = np.array(center)
        
        # 计算向量的模长
        vector_norm = np.linalg.norm(vector)
        if vector_norm == 0:
            return 0.0  # 避免除以零
        
        # X轴单位向量
        x_axis = np.array([1, 0, 0])
        
        # 计算与X轴的夹角（弧度）
        # 使用点积公式：cosθ = (v·w) / (|v|·|w|)
        dot_product = np.dot(vector, x_axis)
        cos_theta = dot_product / vector_norm
        
        # 确保值在[-1, 1]范围内，避免数值误差导致的问题
        cos_theta = np.clip(cos_theta, -1.0, 1.0)
        angle_rad = np.arccos(cos_theta)
        
        # 转换为度并返回
        return np.degrees(angle_rad)

    def detect_collision(self, target_points, other_clusters):
        """
        检测目标长方体与其他聚类之间的碰撞风险
        
        参数:
            target_points: 目标长方体的点云
            other_clusters: 其他聚类的列表
            
        返回:
            collision_detected: 是否检测到碰撞风险
            collision_clusters: 发生碰撞风险的聚类列表
        """
        # 创建目标点云的KDTree，用于高效最近邻搜索
        target_pcd = o3d.geometry.PointCloud()
        target_pcd.points = o3d.utility.Vector3dVector(target_points)
        target_tree = o3d.geometry.KDTreeFlann(target_pcd)
        
        collision_detected = False
        collision_clusters = []
        
        # 检查每个其他聚类与目标的距离
        for cluster in other_clusters:
            cluster_points = cluster['points']
            min_distance = float('inf')
            
            # 对聚类中的采样点进行距离计算（为提高效率，不检查所有点）
            sample_rate = 0.2  # 采样比例
            sample_indices = np.random.choice(
                len(cluster_points), 
                max(1, int(len(cluster_points) * sample_rate)), 
                replace=False
            )
            
            for idx in sample_indices:
                point = cluster_points[idx]
                # 查找目标点云中的最近点
                [k, target_idx, distances] = target_tree.search_knn_vector_3d(point, 1)
                
                if distances[0] < min_distance:
                    min_distance = distances[0]
                
                # 如果已找到小于阈值的距离，提前退出
                if min_distance < self.collision_threshold:
                    break
            
            # 判断是否发生碰撞风险
            if min_distance < self.collision_threshold:
                collision_detected = True
                collision_clusters.append(cluster)
                self.get_logger().info(
                    f"聚类 {cluster['label']} 与目标距离 {min_distance:.4f}m，小于阈值 {self.collision_threshold}m，存在碰撞风险"
                )
        
        return collision_detected, collision_clusters

    def create_pose_message(self, header, center, tilt_angle):
        """创建姿态消息，包含中心点坐标和倾斜角度"""
        pose = PoseStamped()
        pose.header = header
        
        # 设置位置（长方体中心点）- 转换为Python原生float类型
        pose.pose.position.x = float(center[0])
        pose.pose.position.y = float(center[1])
        pose.pose.position.z = float(center[2])
        
        # 将倾斜角度转换为四元数（绕Y轴旋转）
        angle_rad = np.radians(tilt_angle)
        pose.pose.orientation.x = 0.0
        pose.pose.orientation.y = float(np.sin(angle_rad / 2))
        pose.pose.orientation.z = 0.0
        pose.pose.orientation.w = float(np.cos(angle_rad / 2))
        
        return pose

    def hsv_to_rgb(self, h, s, v):
        """HSV转RGB颜色空间"""
        h6 = h * 6
        i = np.floor(h6)
        f = h6 - i
        p = v * (1 - s)
        q = v * (1 - s * f)
        t = v * (1 - s * (1 - f))
        
        i = int(i) % 6
        if i == 0:
            return [v, t, p]
        elif i == 1:
            return [q, v, p]
        elif i == 2:
            return [p, v, t]
        elif i == 3:
            return [p, q, v]
        elif i == 4:
            return [t, p, v]
        else:
            return [v, p, q]

    def create_pointcloud2_msg(self, header, points_array):
        """创建ROS标准PointCloud2消息"""
        pcl_msg = PointCloud2()
        pcl_msg.header = header
        pcl_msg.height = 1
        pcl_msg.width = len(points_array)
        pcl_msg.fields = FIELDS
        pcl_msg.is_bigendian = False
        pcl_msg.point_step = POINT_STEP
        pcl_msg.row_step = POINT_STEP * pcl_msg.width
        pcl_msg.is_dense = False
        
        data = []
        for point in points_array:
            x, y, z, intensity = point
            data.append(struct.pack('fffB', x, y, z, int(intensity)))
        pcl_msg.data = b''.join(data)
        
        return pcl_msg

    def visualization_loop(self):
        """可视化线程主循环"""
        is_first_visualization = True
        
        while self.is_visualizing:
            with self.point_cloud_lock:
                if is_first_visualization and len(self.pcd.points) > 0:
                    self.vis.add_geometry(self.pcd)
                    self.vis.add_geometry(self.target_pcd)
                    self.vis.add_geometry(self.collision_pcd)
                    self.vis.add_geometry(self.line_set)  # 添加连线到可视化窗口
                    
                    ctr = self.vis.get_view_control()
                    ctr.set_front([0, 0, 1])
                    ctr.set_up([0, 1, 0])
                    ctr.set_lookat([0, 0, 0])
                    ctr.set_zoom(0.4)
                    
                    is_first_visualization = False
                elif not is_first_visualization:
                    self.vis.update_geometry(self.pcd)
                    self.vis.update_geometry(self.target_pcd)
                    self.vis.update_geometry(self.collision_pcd)
                    self.vis.update_geometry(self.line_set)  # 更新连线
                    self.vis.poll_events()
                    self.vis.update_renderer()
            
            time.sleep(0.03)

    def save_current_frame(self):
        """保存当前帧点云"""
        filename = f"{self.save_path}/pointcloud_{self.frame_count:06d}.pcd"
        o3d.io.write_point_cloud(filename, self.pcd)
        self.get_logger().info(f"已保存点云: {filename}")

    def destroy_node(self):
        """节点销毁清理"""
        if self.enable_visualization:
            self.is_visualizing = False
            self.vis.destroy_window()
        super().destroy_node()


def main(args=None):
    """程序主入口"""
    rclpy.init(args=args)
    node = LidarProcessor()
    
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
    